/*************************************************************************
  Crytek Source File.
  Copyright (C), Crytek Studios, 2001-2010.
 -------------------------------------------------------------------------
  $Id$
  $DateTime$
  
 -------------------------------------------------------------------------
  History:
  - Oct 5th, 2010  : Created by Michelle Martin
 *************************************************************************/

#include "StdAfx.h"
#include <IEntitySystem.h>		// For IEntity and GetEntity(id)
#include <ICryAnimation.h>		// For SkeletonPose and CharacterInstance interfaces
#include <StringUtils.h>			// for CryStringUtils, to calculate hash values from strings
#include "CryActionCVars.h"   // to make use of ik related CVars

#include "IKTargetSystem.h"
#include "IKTarget_EntityPos.h"
#include "IKTarget_AttachedHelperPos.h"
#include "IKTarget_Attachment.h"
#include "IKTarget_Bone.h"


EntityId CIKTargetSystem::m_debugEntity;


//====================================================================

CIKTargetSystem::CIKTargetSystem()
{
}

CIKTargetSystem::~CIKTargetSystem()
{
	StopAllIK();
}

//====================================================================

void CIKTargetSystem::StopAllIK()
{
	// Delete all active IKs
	// Reroute Callbacks

	ICharacterInstance *pCharacter = NULL;

	TActiveIKs::iterator it = m_activeIKs.begin();
	TActiveIKs::iterator endIt = m_activeIKs.end();
	for ( ; it != endIt; ++it)
	{
		SCharacterIKData* charIKData = it->second;
		
		TActiveCharacterIKs::iterator itIK = charIKData->m_activeCharIKs.begin();
		TActiveCharacterIKs::iterator endItIK = charIKData->m_activeCharIKs.end();
		for ( ; itIK != endItIK; ++itIK)
		{
			itIK->second->StopLimbAnimation(0.0f);
			SAFE_DELETE(itIK->second);
		}

		IEntity* pEntity = gEnv->pEntitySystem->GetEntity(it->first);
		if (pEntity)
		{
			pCharacter = gEnv->pEntitySystem->GetEntity(it->first)->GetCharacter(0);
			if (pCharacter)
			{
				RestoreAnimCallback(pCharacter, charIKData);
				charIKData->m_activeCharIKs.clear();
			}
		}
		SAFE_DELETE(charIKData);
	}

	m_activeIKs.clear();

}

//====================================================================

void CIKTargetSystem::StopAllIKOnCharacter( EntityId entityID )
{
	// Find character in map
	TActiveIKs::iterator it = m_activeIKs.find(entityID);
	if ( it == m_activeIKs.end())
		return;
	SCharacterIKData* charIKData = m_activeIKs[entityID];

	TActiveCharacterIKs::iterator itIK = charIKData->m_activeCharIKs.begin();
	TActiveCharacterIKs::iterator endItIK = charIKData->m_activeCharIKs.end();
	for ( ; itIK != endItIK; ++itIK)
	{
		itIK->second->StopLimbAnimation(0.0f);
		SAFE_DELETE(itIK->second);
	}

	ICharacterInstance* pCharacter = gEnv->pEntitySystem->GetEntity(it->first)->GetCharacter(0);
	RestoreAnimCallback(pCharacter, charIKData);
	charIKData->m_activeCharIKs.clear();
	SAFE_DELETE(charIKData);

	m_activeIKs.erase(it);
}

//====================================================================
// CALLBACK FUNCTION FROM THE ANIMATION SYSTEM
//====================================================================


//this function will be called from the engine at the right time, since bones editing must be placed at the right time.
int UpdateCharacterIK(ICharacterInstance *pCharacter, void *callbackData)
{
	((SIKCallbackData*)callbackData)->ikTargetSystem->UpdateIKforCharacter(((SIKCallbackData*)callbackData)->entityID);

	return 1;
}
//====================================================================
//====================================================================

//====================================================================

void CIKTargetSystem::StopIKonCharacterLimb( EntityId entityID, const char* ikLimbName, float fadeOutTime )
{
	StopIKonCharacterLimb(entityID, *((uint64*)ikLimbName), fadeOutTime);
}

void CIKTargetSystem::StopIKonCharacterLimb( EntityId entityID, uint64 ikLimbHash, float fadeOutTime )
{
	// Find character in map
	if (m_activeIKs.find(entityID) == m_activeIKs.end())
		return;
	SCharacterIKData* charIKData = m_activeIKs[entityID];

	// Find limb in map
	TActiveCharacterIKs::iterator it = charIKData->m_activeCharIKs.find(ikLimbHash);
	if (it == charIKData->m_activeCharIKs.end())
		return;

	// if found, stop IK (the actual removal happens later in the callback)
	(it->second)->FadeOut(fadeOutTime);

}


//====================================================================

bool CIKTargetSystem::ExistsIKTargetForCharacterLimb( EntityId entityID, uint64 ikLimbHash ) 
{
	// Find character in map
	if (m_activeIKs.find(entityID) == m_activeIKs.end())
		return false;

	// Find limb in map
	SCharacterIKData* charIKData = m_activeIKs[entityID];
	TActiveCharacterIKs::iterator it = charIKData->m_activeCharIKs.find(ikLimbHash);
	if (it == charIKData->m_activeCharIKs.end())
		return false;

	return (it->second->GetIKState() != IK_FULLY_FADED_OUT);
}

//====================================================================

bool CIKTargetSystem::IsIKTargetForCharacterLimbFullyBlendedIn( EntityId entityID, uint64 ikLimbHash )
{
	// Find character in map
	if (m_activeIKs.find(entityID) == m_activeIKs.end())
		return false;

	// Find limb in map
	SCharacterIKData* charIKData = m_activeIKs[entityID];
	TActiveCharacterIKs::iterator it = charIKData->m_activeCharIKs.find(ikLimbHash);
	if (it == charIKData->m_activeCharIKs.end())
		return false;

	return (it->second->GetIKState() == IK_FULLY_BLENDED_IN);
}

//====================================================================

int CIKTargetSystem::AddIKTarget_Bone(const SIKTargetParams& ikParams, EntityId targetEntity, const char* boneName )
{
	ICharacterInstance *pCharacter = gEnv->pEntitySystem->GetEntity(targetEntity)->GetCharacter(0);
	ISkeletonPose * pSkeletonPose = pCharacter->GetISkeletonPose();
	int16 boneID = pSkeletonPose->GetJointIDByName(boneName);
	if (boneID < 0)
		return IKErr_No_Valid_Bone;

	return AddIKTarget_Bone(ikParams, targetEntity, (uint)boneID);
}

int CIKTargetSystem::AddIKTarget_Bone(const SIKTargetParams& ikParams, EntityId targetEntity, uint boneId )
{
	// SAFETY CHECKING
	// ==================
	// General safety checking
	int safetyCheckResult = SafetyCheck_IKParams(ikParams);
	if (safetyCheckResult != 0)
		return safetyCheckResult;

	// safety checking specific for this IKTarget type
	IEntity* pTargetEntity = gEnv->pEntitySystem->GetEntity(targetEntity);
	if (!pTargetEntity)
		return IKErr_No_Valid_TargetEntityID;
	ICharacterInstance *pTargetCharacter = pTargetEntity->GetCharacter(0);
	if (!pTargetCharacter)
		return IKErr_No_Valid_Target_Skeleton;
	ISkeletonPose * pTargetSkeletonPose = pTargetCharacter->GetISkeletonPose();
	if (!pTargetSkeletonPose)
		return IKErr_No_Valid_Target_Skeleton;

	// ADD IK TARGET
	// ==================
	SCharacterIKData* charIKData = GetCharacterData(ikParams);
	QuatT currentPos = DeleteCurrentTarget(charIKData, ikParams);

	// Create IK Target for Attached Helpers
	CIKTarget_Bone* pNewIKTarget = new CIKTarget_Bone(ikParams, currentPos, targetEntity, boneId);

	// Add everything
	charIKData->m_activeCharIKs[ikParams.ikHandleHash] = pNewIKTarget;
	m_activeIKs[ikParams.characterEntityID] = charIKData;

	// Re-route Callback from the animation system (if needed)
	// a revive, reset or reload scripts might have caused the callback to be reset or changed
	// (although these cases all should be covered already)
	RerouteAnimCallback(ikParams.characterEntityID, charIKData);

	return IKErr_Ok;  
}

//====================================================================

int CIKTargetSystem::AddIKTarget_Attachment(const SIKTargetParams& ikParams, EntityId targetEntity, const char* attachmentName )
{
	// SAFETY CHECKING
	// ==================
	// General safety checking
	int safetyCheckResult = SafetyCheck_IKParams(ikParams);
	if (safetyCheckResult != 0)
		return safetyCheckResult;

	// safety checking specific for this IKTarget type
	IEntity* pTargetEntity = gEnv->pEntitySystem->GetEntity(targetEntity);
	if (!pTargetEntity)
		return IKErr_No_Valid_TargetEntityID;
	if (!attachmentName || attachmentName == '\0')
		return IKErr_No_Valid_AttachedHelperName;

	// ADD IK TARGET
	// ==================
	SCharacterIKData* charIKData = GetCharacterData(ikParams);
	QuatT currentPos = DeleteCurrentTarget(charIKData, ikParams);

	// Create IK Target for Attached Helpers
	CIKTarget_Attachment* pNewIKTarget = new CIKTarget_Attachment(ikParams, currentPos, targetEntity, attachmentName);

	// Add everything
	charIKData->m_activeCharIKs[ikParams.ikHandleHash] = pNewIKTarget;
	m_activeIKs[ikParams.characterEntityID] = charIKData;

	// Re-route Callback from the animation system (if needed)
	// a revive, reset or reload scripts might have caused the callback to be reset or changed
	// (although these cases all should be covered already)
	RerouteAnimCallback(ikParams.characterEntityID, charIKData);

	return IKErr_Ok;  
}

//====================================================================

int CIKTargetSystem::AddIKTarget_AttachedHelperPos(const SIKTargetParams& ikParams, EntityId targetEntity, const char* helperName )
{
	// SAFETY CHECKING
	// ==================
	// General safety checking
	int safetyCheckResult = SafetyCheck_IKParams(ikParams);
	if (safetyCheckResult != 0)
		return safetyCheckResult;

	// safety checking specific for this IKTarget type
	IEntity* pTargetEntity = gEnv->pEntitySystem->GetEntity(targetEntity);
	if (!pTargetEntity)
		return IKErr_No_Valid_TargetEntityID;
	if (!helperName || CIKTarget_AttachedHelperPos::GetHelperPosFromEntityObject(pTargetEntity, helperName).IsIdentity())
		return IKErr_No_Valid_AttachedHelperName;

	// ADD IK TARGET
	// ==================
	SCharacterIKData* charIKData = GetCharacterData(ikParams);
	QuatT currentPos = DeleteCurrentTarget(charIKData, ikParams);

	// Create IK Target for Attached Helpers
	CIKTarget_AttachedHelperPos* pNewIKTarget = new CIKTarget_AttachedHelperPos(ikParams, currentPos, targetEntity, helperName);

	// Add everything
	charIKData->m_activeCharIKs[ikParams.ikHandleHash] = pNewIKTarget;
	m_activeIKs[ikParams.characterEntityID] = charIKData;

	// Re-route Callback from the animation system (if needed)
	// a revive, reset or reload scripts might have caused the callback to be reset or changed
	// (although these cases all should be covered already)
	RerouteAnimCallback(ikParams.characterEntityID, charIKData);

	return IKErr_Ok;  
}

//====================================================================

int CIKTargetSystem::AddIKTarget_EntityPos(const SIKTargetParams& ikParams, EntityId targetEntity )
{
	// SAFETY CHECKING
	// ==================
	// General safety checking
	int safetyCheckResult = SafetyCheck_IKParams(ikParams);
	if (safetyCheckResult != 0)
		return safetyCheckResult;

	// safety checking specific for this IKTarget type
	IEntity* pTargetEntity = gEnv->pEntitySystem->GetEntity(targetEntity);
	if (!pTargetEntity)
		return IKErr_No_Valid_TargetEntityID;


	// ADD IK TARGET
	// ==================
	SCharacterIKData* charIKData = GetCharacterData(ikParams);
	QuatT currentPos = DeleteCurrentTarget(charIKData, ikParams);

	// Create IK Target EntityPos
	CIKTarget_EntityPos* pNewIKTarget = new CIKTarget_EntityPos(ikParams, currentPos, targetEntity);

	// Add everything
	charIKData->m_activeCharIKs[ikParams.ikHandleHash] = pNewIKTarget;
	m_activeIKs[ikParams.characterEntityID] = charIKData;

	// Re-route Callback from the animation system (if needed)
	// a revive, reset or reload scripts might have caused the callback to be reset or changed
	// (although these cases all should be covered already)
	RerouteAnimCallback(ikParams.characterEntityID, charIKData);


	return IKErr_Ok;
}

//====================================================================

int CIKTargetSystem::SafetyCheck_IKParams(const SIKTargetParams &ikParams )
{
	IEntity* pCharacterEntity = gEnv->pEntitySystem->GetEntity(ikParams.characterEntityID);
	if (!pCharacterEntity)
		return IKErr_No_Valid_EntityID;

	ICharacterInstance* pCharacterInstance = pCharacterEntity->GetCharacter(0);
	if (!pCharacterInstance)
		return IKErr_No_Valid_CharacterInterface;

	if (-1 == pCharacterInstance->GetICharacterModel()->GetICharacterModelSkeleton()->GetLimbDefinitionIdx(ikParams.ikHandleHash))
		return IKErr_IKHandle_Does_Not_Exist;

	return 0;
}

//====================================================================

void CIKTargetSystem::UpdateIKforCharacter( EntityId entityID )
{
	if (CCryActionCVars::Get().ik_targetsystem_enable == 0)
		return;

	float frameTime = gEnv->pTimer->GetFrameTime();

	SCharacterIKData* charIKData = m_activeIKs[entityID];
	ICharacterInstance *pCharacter = gEnv->pEntitySystem->GetEntity(entityID)->GetCharacter(0);
	const SIKTargetCharData* charIKTargetData = pCharacter->GetICharacterModel()->GetICharacterModelSkeleton()->GetCharIKTargetData();

	// go through all active IKs for this character 
	typedef std::map<uint64, TIKTargetIteratorVector> TSpineMap;

	TIKTargetIteratorVector toBeDeleted;
	TSpineMap spineMap;

	TActiveCharacterIKs::iterator it = charIKData->m_activeCharIKs.begin();
	TActiveCharacterIKs::iterator endIt = charIKData->m_activeCharIKs.end();

	// Spine Adjustment (optional)

	// go through all targets and update them to get the latest position
	// and enumerate the desired spine adjustments
	for (int offset = 0; it != endIt; ++it, ++offset)
	{
		// have them update their world position
		CIKTarget_Base* ikTarget = it->second;
		ikTarget->Update(frameTime, m_debugEntity == entityID ? offset : -1);

		// enumerate Spine Involvements
		// if it has a spine limb, add this iterator to the entry
		uint64 spineHash = ikTarget->GetSpinePolicy();
		if (spineHash !=0)
		{
			spineMap[spineHash].push_back(it);
		}
	}

	// go through the list per spine (should usually be max 1 per character, but still..)
	TSpineMap::const_iterator spineIt = spineMap.begin();
	for ( ; spineIt != spineMap.end(); ++spineIt)
	{
		uint64 spineHandle = spineIt->first;
		TIKTargetIteratorVector& affectedIKTargets = (TIKTargetIteratorVector&)*(&(spineIt->second));

		// Check if this spine already exists in the spine list of this character
		TSpineIKData::iterator spineIKIt = charIKData->m_spineIKData.find(spineHandle);

		// if no, create it
		if (spineIKIt == charIKData->m_spineIKData.end())
		{
			// Does this IK Limb exist?
			if (-1 == pCharacter->GetICharacterModel()->GetICharacterModelSkeleton()->GetLimbDefinitionIdx(spineIt->first))
			{
				continue;
			}

			// Create a new entry for the spine list
			SSpineIKData newSpine;
			newSpine.spineTargetWorld.t = CIKTarget_Base::GetLimbEndeffectorWorldPos(entityID, spineHandle).t;
			// blendVals at zero
			newSpine.b1 = newSpine.b2 = 0.0f;
			// targets initialized with final targets
			newSpine.target1 = affectedIKTargets.at(0)->second->GetCurrentTargetWorlPos().t;
			newSpine.limbHandle1 = affectedIKTargets.at(0)->first;
			if (affectedIKTargets.size() > 1)
			{
				newSpine.limbHandle2 = affectedIKTargets.at(1)->first;
				newSpine.target2 = 	affectedIKTargets.at(1)->second->GetCurrentTargetWorlPos().t;
			}

			// push back with the limb hash
			charIKData->m_spineIKData[spineHandle] = newSpine;
		}
		else
		{
			// update it (no blend-out)

			// spine data exists
			SSpineIKData* spineIKData = &(spineIKIt->second);

			// update amount of limbs
			if (affectedIKTargets.size() == 2 && spineIKData->limbHandle2 == 0)
			{
				spineIKData->limbHandle2 = affectedIKTargets.at(1)->first;
				spineIKData->target2 = 	affectedIKTargets.at(1)->second->GetCurrentTargetWorlPos().t;
			}

			// update this spine
			// if the blendVal is non-one blend-out, but never be smaller than original blendVal
			// if the blendVal is 1.0, update the target (over time, but fast)
			float b1 = affectedIKTargets.at(0)->second->GetCurrentBlendVal();
			if (b1 > spineIKData->b1)
				spineIKData->b1 = b1;
			float s = 1.0f;// frameTime / 0.1f; // testsshow that the interpolation here is not actually needed for better visual results
			if (b1 >= 1.0f)
			{
				spineIKData->target1 = spineIKData->target1 * (1.0f - s) + affectedIKTargets.at(0)->second->GetCurrentTargetWorlPos().t * s;
				// remove mark as blend out on blend values that are 1.0f
				spineIKData->b1BlendingOut = false;
			}
			float b2 = 0.0f;
			if (affectedIKTargets.size() > 1)
			{
				b2 = affectedIKTargets.at(1)->second->GetCurrentBlendVal();
				if (b2 > spineIKData->b2)
					spineIKData->b2 = b2;
				if (b2 >= 1.0f)
				{
					spineIKData->target2 = spineIKData->target2 * (1.0f - s) + affectedIKTargets.at(1)->second->GetCurrentTargetWorlPos().t * s;
					// remove mark as blend out on blend values that are 1.0f
					spineIKData->b2BlendingOut = false;
				}
			}
			
			// Allow the spine to be removed from the update queue (this prevents removal on fading in due to small blendvalues)
			if (b1 <= 0.0f && b2 <= 0.0f)
				spineIKData->canBeRemoved = true;
		}
	}

	// Update all the character's spines (blend-out only) and execute
	TSpineIKData::iterator spineIKIt = charIKData->m_spineIKData.begin();
	for ( ; spineIKIt != charIKData->m_spineIKData.end(); )
	{
		SSpineIKData* spineIKData = &(spineIKIt->second);

		// POSSIBLE IMPROVEMENT: Check that the spine is not itself present in an IK Target (in that case, don't execute, remove this)

		// Blend-out if blend values are marked for it
		if (spineIKData->b1BlendingOut)
		{
			spineIKData->b1 = spineIKData->b1 - (frameTime / 4.0f);
		}
		if (spineIKData->b2BlendingOut)
		{
			spineIKData->b2 = spineIKData->b2 - (frameTime / 4.0f);
		}

		// If both blend-values are <=zero, remove spine from list and continue
		if (spineIKData->canBeRemoved && spineIKData->b1 <= 0.1f && spineIKData->b2 <= 0.1f)
		{
			charIKData->m_spineIKData.erase(spineIKIt);
			spineIKIt = charIKData->m_spineIKData.begin(); // technically this is not correct and can lead to double updates, but will work fastest in >99% of all cases
			continue;
		}

		// mark both blend value as blend-out (if IK still active, it will reset this markup
		spineIKData->b2BlendingOut = true;
		spineIKData->b1BlendingOut = true;


		QuatT spineTarget;

		// execute
		uint64 spineHandle = spineIKIt->first;
		spineTarget = CalculateSpineTarget_Type02(entityID, spineHandle, spineIKData, charIKTargetData);

		// POSSIBLE IMPROVEMENT: Spine rotation (to cover special targets that are a good deal off to the side)

		// interpolate world position of the spine target for better visual quality
		float s = min(1.0f, frameTime / 0.065f); // value found empirically
		spineIKData->spineTargetWorld.t = spineIKData->spineTargetWorld.t * (1.0f - s) + spineTarget.t * s;
		
		// NOTE: If there is rotation, interpolate instead in the line below
		spineIKData->spineTargetWorld.q = spineTarget.q;

		// POSSIBLE IMPROVEMENT: if target has "fix limb" option, fix that limb if it does not also have an IK target itself

		// call spine ik
		pCharacter->GetISkeletonPose()->SetHumanLimbIK_ExecuteImmediately(spineTarget, spineIKIt->first, true);

		++spineIKIt;
	}


	// trigger the limb IKs with the updated position
	it = charIKData->m_activeCharIKs.begin();
	for (int offset = 0; it != endIt; ++it, ++offset)
	{
		// have them update their world position
		CIKTarget_Base* ikTarget = it->second;
		//ikTarget->Update(frameTime, m_debugEntity == entityID ? offset : -1);
		QuatT curWPos = ikTarget->GetCurrentWorldPos();

		// only send the "true" parameter if the end effector should be adjusted
		bool alignEndEffector = ikTarget->GetEndEffectorAlignPolicy();

		// Execute the IKs
		pCharacter->GetISkeletonPose()->SetHumanLimbIK_ExecuteImmediately(curWPos, it->first, alignEndEffector);

		// Check if IKs are done and add the to the remove list
		if (ikTarget->GetIKState() == IK_FULLY_FADED_OUT)
			toBeDeleted.push_back(it);
	}


	// Call original callback for this character
	if (charIKData->m_pOriginalPostProcessCallback)		
		(*charIKData->m_pOriginalPostProcessCallback)(pCharacter, charIKData->m_pOriginalPostProcessCallbackData);


	// Delete IKs no longer needed
	if (!toBeDeleted.empty())
	{
		std::vector<TActiveCharacterIKs::iterator>::iterator deletionIt = toBeDeleted.begin();
		for ( ; deletionIt != toBeDeleted.end(); ++deletionIt)
		{
			// Remove IKTarget for this char
			it = *deletionIt;
			SAFE_DELETE(it->second);
			charIKData->m_activeCharIKs.erase(it);
		}
	}

	// If the last one IK is removed AND no more spine is active, remove the character
	// and restore the callback function
	if (charIKData->m_activeCharIKs.empty() && charIKData->m_spineIKData.empty())
	{
		m_activeIKs.erase(m_activeIKs.find(entityID));
		RestoreAnimCallback(pCharacter, charIKData);
	}
}

//====================================================================

CIKTargetSystem::SCharacterIKData* CIKTargetSystem::GetCharacterData(const SIKTargetParams &ikParams )
{
	// Add character to map if needed
	SCharacterIKData* charIKData = NULL;
	if (m_activeIKs.find(ikParams.characterEntityID) != m_activeIKs.end())
		charIKData = m_activeIKs[ikParams.characterEntityID];
	else
	{
		charIKData = new SCharacterIKData();
	}

	return charIKData;
}

//====================================================================

QuatT CIKTargetSystem::DeleteCurrentTarget( SCharacterIKData* charIKData,const SIKTargetParams &ikParams )
{
	QuatT currentPos = QuatT(IDENTITY);
	// check if handle already active
	if (charIKData->m_activeCharIKs.find(ikParams.ikHandleHash) != charIKData->m_activeCharIKs.end())
	{
		// if previous IK Target for this handle exists, use its current world pos as starting position 
		// (otherwise the IKTarget will use the end effectors position as provided by the animation update)
		CIKTarget_Base* currentTarget = charIKData->m_activeCharIKs[ikParams.ikHandleHash];
		currentPos = currentTarget->GetCurrentWorldPos();
		SAFE_DELETE(currentTarget);
	}
	return currentPos;
}

//====================================================================

void CIKTargetSystem::WriteErrorMessage( int errorCode )
{
	switch (errorCode)
	{
		case IKErr_Ok:
			CryLogAlways("IK Target System: No Errors.");
			break;
		case IKErr_No_Valid_EntityID:
			CryLogAlways("IK Target System: No Valid Entity for Character supplied. IK will not be initiated.");
			break;
		case IKErr_No_Valid_CharacterInterface:
			CryLogAlways("IK Target System: Entity supplied is not a character. IK will not be initiated.");
			break;
		case IKErr_No_Valid_TargetEntityID:
			CryLogAlways("IK Target System: Target Entity supplied is invalid. IK will not be initiated.");
			break;
		case IKErr_IKHandle_Does_Not_Exist:
			CryLogAlways("IK Target System: The specified IK Handle is not declared in this character's IK Setup. Check IK File or chrparams. IK will not be initiated.");
			break;
		case IKErr_No_Valid_AttachedHelperName:
			CryLogAlways("IK Target System: No Valid Attachment or Helper name specified, or helper couldn't be found on this entity. IK will not be initiated.");
			break;
		case IKErr_No_Valid_Bone:
			CryLogAlways("IK Target System: The bone name provided could not be found. IK will not be initiated.");
			break;
		case IKErr_No_Valid_Target_Skeleton:
			CryLogAlways("IK Target System: Target Entity is not a bone-based character, but bone target has been specified. IK will not be initiated.");
			break;
		default:
			CryLogAlways("IK Target System: Failed to initiate IK for unknown reason. Please extend this message function. Undocumented Error Code: %i", errorCode);
			break;
	}

	// If this assert triggers, most likely you added more error codes to EIK_ErrorCodes but you didn't update this function.
	// Do so now and increase the number in the assert to match the number of entries in the struct.
	COMPILE_TIME_ASSERT(IKErr_MaxDocumented == 8);

}

//====================================================================

void CIKTargetSystem::RerouteAnimCallback( EntityId entityID, SCharacterIKData* charIKData )
{
	// Check whether the current callback is set to this callback - reroute otherwise
	IEntity* pCharacterEntity = gEnv->pEntitySystem->GetEntity(entityID);
	ICharacterInstance *pCharacter = pCharacterEntity->GetCharacter(0);
	int (*tempFuncPointer)(ICharacterInstance*,void*) = NULL;
	void* tempFuncData = NULL;

	tempFuncPointer = (int (__cdecl *)(ICharacterInstance *,void *))pCharacter->GetISkeletonPose()->GetPostProcessCallback(tempFuncData);

	// Reroute if it hasn't been already done
	if (tempFuncPointer != UpdateCharacterIK)
	{
		charIKData->m_pOriginalPostProcessCallback = tempFuncPointer;
		charIKData->m_pOriginalPostProcessCallbackData = tempFuncData;
		charIKData->m_callbackData.Set(entityID, this);
		pCharacter->GetISkeletonPose()->SetPostProcessCallback(UpdateCharacterIK, &(charIKData->m_callbackData));
//		CryLogAlways("Rerouted Callback");
	}
}

void CIKTargetSystem::RestoreAnimCallback( ICharacterInstance * pCharacter, SCharacterIKData* charIKData )
{
	pCharacter->GetISkeletonPose()->SetPostProcessCallback(charIKData->m_pOriginalPostProcessCallback, charIKData->m_pOriginalPostProcessCallbackData);
}

//====================================================================

void CIKTargetSystem::ChangeDebugEntity( EntityId entityID )
{
	m_debugEntity = entityID;
}


//====================================================================

QuatT CIKTargetSystem::CalculateSpineTarget_Type02( EntityId entityID, uint64 spineHandle, SSpineIKData* spineIKData, const SIKTargetCharData* charIKTargetData )
{
	QuatT spineNeckLocation = CIKTarget_Base::GetLimbEndeffectorWorldPos(entityID, spineHandle);
	QuatT pelvisLoc = CIKTarget_Base::GetLimbRootWorldPos(entityID, spineHandle);
	Vec3 blendStartPos = spineNeckLocation.t; //pelvisPos + (spineNeckPos - pelvisPos) * 0.3f; //spineNeckPos;

	QuatT target;
	target = spineNeckLocation;

	// get the targets
	//Vec3 T1 = affectedIKTargets.at(0)->second->GetCurrentTargetWorlPos().t;
	Vec3 T1 = spineIKData->target1;
	//float b1 = affectedIKTargets.at(0)->second->GetCurrentBlendVal();
	float b1 = spineIKData->b1;


	float smallerBlendVal = b1;
	T1 = blendStartPos + (T1 - blendStartPos) * b1;
	Vec3 T2 = blendStartPos; // T1
	float b2 = 0.0f;
	
	// get the roots of the first two limbs (use spineNeck if there is only one)
	QuatT R1 = CIKTarget_Base::GetLimbRootWorldPos(entityID, spineIKData->limbHandle1);
	QuatT R2 = spineNeckLocation;

	int rootCount = (spineIKData->limbHandle2 != 0) ? 2 : 1;
	if (rootCount > 1)
	{
		//T2 = affectedIKTargets.at(1)->second->GetCurrentTargetWorlPos().t;
		T2 = spineIKData->target2;
		//b2 = affectedIKTargets.at(1)->second->GetCurrentBlendVal();
		b2 = spineIKData->b2;
		T2 = blendStartPos + (T2 - blendStartPos) * b2;
		R2 = CIKTarget_Base::GetLimbRootWorldPos(entityID, spineIKData->limbHandle2);
	}
	if (b2 < smallerBlendVal)
		smallerBlendVal = b2;


	// get the middle point (depending on one target, two target, or fix hand
	Vec3 M = T2 + (T1 - T2) * (0.5f);
	// DEBUG DRAWING
	if (m_debugEntity == entityID)
	{
		gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(M, 0.05f, ColorB(255, 0, 0)); // M in Green
	}

/*
	// NOTE: ROTATE the spine first (within a few degrees of the vector pointing from neck to M)
	// --> then recalculate R1 and R2 
	Vec3 neckToM_Norm = (M - spineNeckLocation.t).GetNormalized();
	Vec3 neckUp_Norm = (spineNeckLocation.q * Vec3(0, 0, 1));
	Vec3 neckFwd_Norm = (spineNeckLocation.q * FORWARD_DIRECTION);
	float angle = acosf(neckFwd_Norm * neckToM_Norm); // - DEG2RAD(20.0f);
	if (angle > 0.0f)
	{
		Vec3 neckAxis = neckFwd_Norm % neckToM_Norm;
		Quat newRot = Quat::CreateRotationAA(angle, neckFwd_Norm.GetNormalized());
		target.q = spineNeckLocation.q * newRot;
		Vec3 neckToR1 = R1.t - spineNeckLocation.t;
		Vec3 neckToR2 = R2.t - spineNeckLocation.t;
		R1.t = spineNeckLocation.t + newRot * neckToR1;
		R2.t = spineNeckLocation.t + newRot * neckToR2;
	}
	else
		target.q = spineNeckLocation.q;

*/


	// Actual character setup values are used for these calculations

	// max distances for each limb:
	float maxDist1 = charIKTargetData->m_armLengthDefault; //0.55f;
	std::map<uint64, float>::const_iterator it;
	if (charIKTargetData->m_armLengthValues.end() != (it = charIKTargetData->m_armLengthValues.find(spineIKData->limbHandle1)))
		maxDist1 = it->second;
	float maxDist2 = charIKTargetData->m_armLengthDefault; //0.55f;
	if (charIKTargetData->m_armLengthValues.end() != (it = charIKTargetData->m_armLengthValues.find(spineIKData->limbHandle2)))
		maxDist2 = it->second;
	

	maxDist1 = maxDist1 + smallerBlendVal * 0.05f; // make them a few centimeters longer if a second target comes into play
	maxDist2 = maxDist2 + smallerBlendVal * 0.05f;
	// square them for faster distance comparison
	maxDist1 = maxDist1 * maxDist1;
	maxDist2 = maxDist2 * maxDist2;

	// Pre-Test - maybe the spine needs no modification at all
	// are the roots of both limbs within the spheres?
	if (0.0f == AreRootsWithinTargetSpheres(R1.t, T1, maxDist1, R2.t, T2, maxDist2, rootCount))
		return spineNeckLocation;


	// OK, time to move the spine and see if the spheres can be reached


	// Calculate the remaining angle left to the 90 degrees
	Vec3 fwdAxis = (pelvisLoc * FORWARD_DIRECTION).GetNormalized();
	Vec3 upAxis = (pelvisLoc * Vec3(0,0,1)).GetNormalized();
	Vec3 pelvisToNeck = spineNeckLocation.t - pelvisLoc.t;
	Vec3 pelvisToNeckNorm = pelvisToNeck.GetNormalized();
	Vec3 pelvisToM = M - pelvisLoc.t;
	Vec3 neckToR1 = R1.t - spineNeckLocation.t;
	Vec3 neckToR2 = R2.t - spineNeckLocation.t;
	Vec3 tempAxis = (fwdAxis + upAxis).GetNormalized();
	float maxAngle = acosf(tempAxis * pelvisToNeck.GetNormalized());
	Vec3 rotationAxis = (pelvisToNeck % pelvisToM).GetNormalized();

	// spine values
	float spineLengthMaxFront = charIKTargetData->m_spineLength;   // bending forward reduces spine to this(this way we trick the IK into a curved, natural back)
	float spineLength = pelvisToNeck.GetLength();


	// FORWARD BENDING


	// divide it into XX steps and calculate a delta
	float deltaAlpha = maxAngle / 6.0f; // the higher the step count, the finer the results, >= 4 for finer results
	float alpha = 0.0f;
	float bestAlpha = 0.0f;
	
	// can the next section reach the target
	// calculate the new root positions
	Vec3 testR1, testR2;
	Vec3 pelvisToR1, pelvisToR2;
	bool canReach = false;
	float smallestDist = 100000.0f;
	int i = 0;
	for ( ; alpha <= maxAngle; alpha += deltaAlpha)
	{
		++i;
		// calculate new spine length
		float newSpineLength = spineLength + ((spineLengthMaxFront - spineLength) * (alpha / maxAngle));
		pelvisToR1 = pelvisToNeckNorm * spineLength + neckToR1;
		pelvisToR2 = pelvisToNeckNorm * spineLength + neckToR2;
		// calculate new R1 and R2 positions
		testR1 = pelvisLoc.t + Quat::CreateRotationAA(alpha, rotationAxis) * pelvisToR1;
		testR2 = pelvisLoc.t + Quat::CreateRotationAA(alpha, rotationAxis) * pelvisToR2;
		float dist = AreRootsWithinTargetSpheres(testR1, T1, maxDist1, testR2, T2, maxDist2, rootCount);
		if (dist < smallestDist)
		{
			smallestDist = dist;
			bestAlpha = alpha;
		}
		canReach = (0.0f == dist);
		if (!canReach)
			continue;

		bestAlpha = alpha;
		break;
	}


	// if it can reach now (but couldn't previously) we got a pair
	// now let's narrow it down
	float topAlpha = alpha - deltaAlpha;
	// XX iterations to find a better angle
	for (int j = 0; j < 25; ++j)
	{
		float testAlpha = topAlpha + (bestAlpha - topAlpha) * 0.5f; // halfway between the current best alpha and the top one (gets more accurate with every iteration)
		// calculate new spine length
		float newSpineLength = spineLength + ((spineLengthMaxFront - spineLength) * (testAlpha / maxAngle));
		pelvisToR1 = pelvisToNeckNorm * spineLength + neckToR1;
		pelvisToR2 = pelvisToNeckNorm * spineLength + neckToR2;
		// calculate new R1 and R2 positions
		testR1 = pelvisLoc.t + Quat::CreateRotationAA(testAlpha, rotationAxis) * pelvisToR1;
		testR2 = pelvisLoc.t + Quat::CreateRotationAA(testAlpha, rotationAxis) * pelvisToR2;
		if (0.0f == AreRootsWithinTargetSpheres(testR1, T1, maxDist1, testR2, T2, maxDist2, rootCount))
			bestAlpha = testAlpha;
		else
			topAlpha = testAlpha;
	}

	// POSSIBLE IMPROVEMENT: Try backwards reaching if forward was unsuccessful


	// got a good alpha --> turn it into a spine target position
	// calculate new spine length
	float newSpineLength = spineLength + ((spineLengthMaxFront - spineLength) * (bestAlpha / maxAngle));
	Quat tempRot = Quat::CreateRotationAA(bestAlpha, rotationAxis);
	target.t = pelvisLoc.t + Quat::CreateRotationAA(bestAlpha, rotationAxis) * pelvisToNeck; // (pelvisToNeckNorm * newSpineLength);
	//target.q = tempRot * spineNeckLocation.q;

	if (m_debugEntity == entityID)
	{
		gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(testR1, 0.05f, ColorB(255,0,0)); 
		gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(testR2, 0.05f, ColorB(255,0,0)); 
		gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(target.t, 0.05f, ColorB(255,255,0));
	}

	return target;
}

//====================================================================

float CIKTargetSystem::AreRootsWithinTargetSpheres( Vec3 R1, Vec3 T1, float maxDist1, Vec3 R2, Vec3 T2, float maxDist2, int rootCount )
{
	float dist1 = (T1 - R1).GetLengthSquared();
	float dist2 = 0;
	if (rootCount > 1)
		dist2 = (T2 - R2).GetLengthSquared();
	if (dist1 <= maxDist1)
	{
		if (dist2 <= maxDist2)
			return 0.0f;
	}
	return (dist1 + dist2);
}

//====================================================================

CIKTarget_Base* CIKTargetSystem::GetIKTargetForCharacterLimb( EntityId entityID, uint64 ikLimbHash ) 
{
	// Find character in map
	if (m_activeIKs.find(entityID) == m_activeIKs.end())
		return NULL;

	// Find limb in map
	SCharacterIKData* charIKData = m_activeIKs[entityID];
	TActiveCharacterIKs::iterator it = charIKData->m_activeCharIKs.find(ikLimbHash);
	if (it == charIKData->m_activeCharIKs.end())
		return NULL;

	return it->second;
}

//====================================================================

void CIKTargetSystem::SetOffsetOnTarget( EntityId entityID, uint64 ikLimbHash, QuatT newOffset )
{
	CIKTarget_Base* ikTarget = GetIKTargetForCharacterLimb(entityID, ikLimbHash);
	
	if (ikTarget)
		ikTarget->SetOffset(newOffset);
}

//====================================================================





